/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include <stdint.h>

#include <stdint.h>
#include <stdio.h>
#include "rtthread.h"
#include <string.h>

static uint8_t ano_tc_tx_buffer[256];
// 计算和校验与附加校验
void ano_tc_cal_sumcheck_and_addcheck(uint8_t *sumcheck, uint8_t *addcheck)
{
    *sumcheck = 0;
    *addcheck = 0;
    for (uint8_t i = 0; i < (ano_tc_tx_buffer[3] + 4); i++)
    {
        *sumcheck += ano_tc_tx_buffer[i];
        *addcheck += *sumcheck;
    }
    *sumcheck &= 0xFF;
    *addcheck &= 0xFF;
}

// 发送数据帧
void ano_tc_send_frame(uint8_t len)
{
    rt_kput((const char *)ano_tc_tx_buffer, len);
}

// 发送惯性传感器数据
void ano_tc_send_imu_data(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, uint8_t shock_sta)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x01;
    ano_tc_tx_buffer[3] = 13;
    // 填充数据内容，小端模式
    ano_tc_tx_buffer[4] = ax & 0xFF;
    ano_tc_tx_buffer[5] = ax >> 8;
    ano_tc_tx_buffer[6] = ay & 0xFF;
    ano_tc_tx_buffer[7] = ay >> 8;
    ano_tc_tx_buffer[8] = az & 0xFF;
    ano_tc_tx_buffer[9] = az >> 8;
    ano_tc_tx_buffer[10] = gx & 0xFF;
    ano_tc_tx_buffer[11] = gx >> 8;
    ano_tc_tx_buffer[12] = gy & 0xFF;
    ano_tc_tx_buffer[13] = gy >> 8;
    ano_tc_tx_buffer[14] = gz & 0xFF;
    ano_tc_tx_buffer[15] = gz >> 8;
    ano_tc_tx_buffer[16] = shock_sta;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[17], &ano_tc_tx_buffer[18]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送罗盘、气压、温度传感器数据
void ano_tc_send_compass_baro_temp_data(int16_t mag_x, int16_t mag_y, int16_t mag_z, int32_t alt_bar, int16_t tmp, uint8_t bar_sta, uint8_t mag_sta)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x02;
    ano_tc_tx_buffer[3] = 14;
    ano_tc_tx_buffer[4] = mag_x & 0xFF;
    ano_tc_tx_buffer[5] = mag_x >> 8;
    ano_tc_tx_buffer[6] = mag_y & 0xFF;
    ano_tc_tx_buffer[7] = mag_y >> 8;
    ano_tc_tx_buffer[8] = mag_z & 0xFF;
    ano_tc_tx_buffer[9] = mag_z >> 8;
    ano_tc_tx_buffer[10] = alt_bar & 0xFF;
    ano_tc_tx_buffer[11] = alt_bar >> 8;
    ano_tc_tx_buffer[12] = alt_bar >> 16;
    ano_tc_tx_buffer[13] = alt_bar >> 24;
    ano_tc_tx_buffer[14] = tmp & 0xFF;
    ano_tc_tx_buffer[15] = tmp >> 8;
    ano_tc_tx_buffer[16] = bar_sta;
    ano_tc_tx_buffer[17] = mag_sta;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[18], &ano_tc_tx_buffer[19]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送飞控姿态（欧拉角格式）数据
void ano_tc_send_flight_control_attitude_euler(int16_t rol, int16_t pit, int16_t yaw, uint8_t fusion_sta)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x03;
    ano_tc_tx_buffer[3] = 7;
    ano_tc_tx_buffer[4] = rol & 0xFF;
    ano_tc_tx_buffer[5] = rol >> 8;
    ano_tc_tx_buffer[6] = pit & 0xFF;
    ano_tc_tx_buffer[7] = pit >> 8;
    ano_tc_tx_buffer[8] = yaw & 0xFF;
    ano_tc_tx_buffer[9] = yaw >> 8;
    ano_tc_tx_buffer[10] = fusion_sta;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[11], &ano_tc_tx_buffer[12]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送飞控姿态（四元数格式）数据
void ano_tc_send_flight_control_attitude_quaternion(int16_t v0, int16_t v1, int16_t v2, int16_t v3, uint8_t fusion_sta)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x04;
    ano_tc_tx_buffer[3] = 9;
    ano_tc_tx_buffer[4] = v0 & 0xFF;
    ano_tc_tx_buffer[5] = v0 >> 8;
    ano_tc_tx_buffer[6] = v1 & 0xFF;
    ano_tc_tx_buffer[7] = v1 >> 8;
    ano_tc_tx_buffer[8] = v2 & 0xFF;
    ano_tc_tx_buffer[9] = v2 >> 8;
    ano_tc_tx_buffer[10] = v3 & 0xFF;
    ano_tc_tx_buffer[11] = v3 >> 8;
    ano_tc_tx_buffer[12] = fusion_sta;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[13], &ano_tc_tx_buffer[14]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送高度数据
void ano_tc_send_height_data(uint8_t alt_sta, int32_t alt_fu, int32_t alt_add)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x05;
    ano_tc_tx_buffer[3] = 9;
    ano_tc_tx_buffer[4] = alt_sta;
    ano_tc_tx_buffer[5] = alt_fu & 0xFF;
    ano_tc_tx_buffer[6] = alt_fu >> 8;
    ano_tc_tx_buffer[7] = alt_fu >> 16;
    ano_tc_tx_buffer[8] = alt_fu >> 24;
    ano_tc_tx_buffer[9] = alt_add & 0xFF;
    ano_tc_tx_buffer[10] = alt_add >> 8;
    ano_tc_tx_buffer[11] = alt_add >> 16;
    ano_tc_tx_buffer[12] = alt_add >> 24;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[13], &ano_tc_tx_buffer[14]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送飞控运行模式数据
void ano_tc_send_flight_control_mode(uint8_t mode, uint8_t sflag, uint8_t cid, uint8_t cmd0, uint8_t cmd1)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x06;
    ano_tc_tx_buffer[3] = 5;
    ano_tc_tx_buffer[4] = mode;
    ano_tc_tx_buffer[5] = sflag;
    ano_tc_tx_buffer[6] = cid;
    ano_tc_tx_buffer[7] = cmd0;
    ano_tc_tx_buffer[8] = cmd1;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[9], &ano_tc_tx_buffer[10]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送飞行速度数据
void ano_tc_send_flight_speed_data(int16_t speed_x, int16_t speed_y, int16_t speed_z)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x07;
    ano_tc_tx_buffer[3] = 6;
    ano_tc_tx_buffer[4] = speed_x & 0xFF;
    ano_tc_tx_buffer[5] = speed_x >> 8;
    ano_tc_tx_buffer[6] = speed_y & 0xFF;
    ano_tc_tx_buffer[7] = speed_y >> 8;
    ano_tc_tx_buffer[8] = speed_z & 0xFF;
    ano_tc_tx_buffer[9] = speed_z >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[10], &ano_tc_tx_buffer[11]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送位置偏移数据
void ano_tc_send_position_offset_data(int32_t pos_x, int32_t pos_y)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x08;
    ano_tc_tx_buffer[3] = 8;
    ano_tc_tx_buffer[4] = pos_x & 0xFF;
    ano_tc_tx_buffer[5] = pos_x >> 8;
    ano_tc_tx_buffer[6] = pos_x >> 16;
    ano_tc_tx_buffer[7] = pos_x >> 24;
    ano_tc_tx_buffer[8] = pos_y & 0xFF;
    ano_tc_tx_buffer[9] = pos_y >> 8;
    ano_tc_tx_buffer[10] = pos_y >> 16;
    ano_tc_tx_buffer[11] = pos_y >> 24;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[12], &ano_tc_tx_buffer[13]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送风速估计数据
void ano_tc_send_wind_speed_estimation(int16_t wind_x, int16_t wind_y)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x09;
    ano_tc_tx_buffer[3] = 4;
    ano_tc_tx_buffer[4] = wind_x & 0xFF;
    ano_tc_tx_buffer[5] = wind_x >> 8;
    ano_tc_tx_buffer[6] = wind_y & 0xFF;
    ano_tc_tx_buffer[7] = wind_y >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8], &ano_tc_tx_buffer[9]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送目标姿态数据
void ano_tc_send_target_attitude_data(int16_t tar_rol, int16_t tar_pit, int16_t tar_yaw)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0A;
    ano_tc_tx_buffer[3] = 6;
    ano_tc_tx_buffer[4] = tar_rol & 0xFF;
    ano_tc_tx_buffer[5] = tar_rol >> 8;
    ano_tc_tx_buffer[6] = tar_pit & 0xFF;
    ano_tc_tx_buffer[7] = tar_pit >> 8;
    ano_tc_tx_buffer[8] = tar_yaw & 0xFF;
    ano_tc_tx_buffer[9] = tar_yaw >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[10], &ano_tc_tx_buffer[11]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送目标速度数据
void ano_tc_send_target_speed_data(int16_t tar_speed_x, int16_t tar_speed_y, int16_t tar_speed_z)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0B;
    ano_tc_tx_buffer[3] = 6;
    ano_tc_tx_buffer[4] = tar_speed_x & 0xFF;
    ano_tc_tx_buffer[5] = tar_speed_x >> 8;
    ano_tc_tx_buffer[6] = tar_speed_y & 0xFF;
    ano_tc_tx_buffer[7] = tar_speed_y >> 8;
    ano_tc_tx_buffer[8] = tar_speed_z & 0xFF;
    ano_tc_tx_buffer[9] = tar_speed_z >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[10], &ano_tc_tx_buffer[11]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送回航信息数据
void ano_tc_send_return_home_info(int16_t r_a, uint16_t r_d)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0C;
    ano_tc_tx_buffer[3] = 4;
    ano_tc_tx_buffer[4] = r_a & 0xFF;
    ano_tc_tx_buffer[5] = r_a >> 8;
    ano_tc_tx_buffer[6] = r_d & 0xFF;
    ano_tc_tx_buffer[7] = r_d >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8], &ano_tc_tx_buffer[9]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送电压电流数据
void ano_tc_send_voltage_current_data(uint16_t voltage, uint16_t current)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0D;
    ano_tc_tx_buffer[3] = 4;
    ano_tc_tx_buffer[4] = voltage & 0xFF;
    ano_tc_tx_buffer[5] = voltage >> 8;
    ano_tc_tx_buffer[6] = current & 0xFF;
    ano_tc_tx_buffer[7] = current >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8], &ano_tc_tx_buffer[9]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送外接模块工作状态数据
void ano_tc_send_external_module_status(uint8_t sta_g_vel, uint8_t sta_g_pos, uint8_t sta_gps, uint8_t sta_alt_add)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0E;
    ano_tc_tx_buffer[3] = 4;
    ano_tc_tx_buffer[4] = sta_g_vel;
    ano_tc_tx_buffer[5] = sta_g_pos;
    ano_tc_tx_buffer[6] = sta_gps;
    ano_tc_tx_buffer[7] = sta_alt_add;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8], &ano_tc_tx_buffer[9]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送RGB亮度信息输出数据
void ano_tc_send_rgb_brightness_info(uint8_t bri_r, uint8_t bri_g, uint8_t bri_b, uint8_t bri_a)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x0F;
    ano_tc_tx_buffer[3] = 4;
    ano_tc_tx_buffer[4] = bri_r;
    ano_tc_tx_buffer[5] = bri_g;
    ano_tc_tx_buffer[6] = bri_b;
    ano_tc_tx_buffer[7] = bri_a;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8], &ano_tc_tx_buffer[9]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送LOG信息输出--字符串数据
void ano_tc_send_log_info_string(uint8_t color, const char *str)
{
    uint8_t len = strlen(str) + 1;
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0xA0;
    ano_tc_tx_buffer[3] = len;
    ano_tc_tx_buffer[4] = color;
    for (uint8_t i = 0; i < strlen(str); i++)
    {
        ano_tc_tx_buffer[5 + i] = str[i];
    }
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[5 + len - 1], &ano_tc_tx_buffer[6 + len - 1]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送LOG信息输出--字符串+数字数据
void ano_tc_send_log_info_string_number(int32_t val, const char *str)
{
    uint8_t len = strlen(str) + 4 + 1;
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0xA1;
    ano_tc_tx_buffer[3] = len;
    ano_tc_tx_buffer[4] = (val & 0xFF);
    ano_tc_tx_buffer[5] = (val >> 8) & 0xFF;
    ano_tc_tx_buffer[6] = (val >> 16) & 0xFF;
    ano_tc_tx_buffer[7] = (val >> 24) & 0xFF;
    for (uint8_t i = 0; i < strlen(str); i++)
    {
        ano_tc_tx_buffer[8 + i] = str[i];
    }
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[8 + len - 4 - 1], &ano_tc_tx_buffer[9 + len - 4 - 1]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送图像数据
void ano_tc_send_image_data(uint16_t flag, uint8_t color, uint16_t begin_row, uint16_t width, const uint8_t *pix_data, uint16_t pix_data_len)
{
    uint16_t total_len = 5 + pix_data_len;
    ano_tc_tx_buffer[0] = 0xAB; // 图像数据帧头
    ano_tc_tx_buffer[1] = 0xAF; // 假设目标地址
    ano_tc_tx_buffer[2] = 0xB0;
    ano_tc_tx_buffer[3] = total_len & 0xFF;
    ano_tc_tx_buffer[4] = total_len >> 8;
    ano_tc_tx_buffer[5] = flag & 0xFF;
    ano_tc_tx_buffer[6] = flag >> 8;
    ano_tc_tx_buffer[7] = color;
    ano_tc_tx_buffer[8] = begin_row & 0xFF;
    ano_tc_tx_buffer[9] = begin_row >> 8;
    ano_tc_tx_buffer[10] = width & 0xFF;
    ano_tc_tx_buffer[11] = width >> 8;
    for (uint16_t i = 0; i < pix_data_len; i++)
    {
        ano_tc_tx_buffer[12 + i] = pix_data[i];
    }
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[12 + pix_data_len - 1], &ano_tc_tx_buffer[13 + pix_data_len - 1]);
    ano_tc_send_frame(total_len + 5);
}

// 发送PWM控制量数据
void ano_tc_send_pwm_control(uint16_t pwm1, uint16_t pwm2, uint16_t pwm3, uint16_t pwm4, uint16_t pwm5, uint16_t pwm6, uint16_t pwm7, uint16_t pwm8)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x20;
    ano_tc_tx_buffer[3] = 16;
    ano_tc_tx_buffer[4] = pwm1 & 0xFF;
    ano_tc_tx_buffer[5] = pwm1 >> 8;
    ano_tc_tx_buffer[6] = pwm2 & 0xFF;
    ano_tc_tx_buffer[7] = pwm2 >> 8;
    ano_tc_tx_buffer[8] = pwm3 & 0xFF;
    ano_tc_tx_buffer[9] = pwm3 >> 8;
    ano_tc_tx_buffer[10] = pwm4 & 0xFF;
    ano_tc_tx_buffer[11] = pwm4 >> 8;
    ano_tc_tx_buffer[12] = pwm5 & 0xFF;
    ano_tc_tx_buffer[13] = pwm5 >> 8;
    ano_tc_tx_buffer[14] = pwm6 & 0xFF;
    ano_tc_tx_buffer[15] = pwm6 >> 8;
    ano_tc_tx_buffer[16] = pwm7 & 0xFF;
    ano_tc_tx_buffer[17] = pwm7 >> 8;
    ano_tc_tx_buffer[18] = pwm8 & 0xFF;
    ano_tc_tx_buffer[19] = pwm8 >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[20], &ano_tc_tx_buffer[21]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送姿态控制量数据
void ano_tc_send_attitude_control(int16_t ctrl_rol, int16_t ctrl_pit, int16_t ctrl_thr, int16_t ctrl_yaw)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x21;
    ano_tc_tx_buffer[3] = 8;
    ano_tc_tx_buffer[4] = ctrl_rol & 0xFF;
    ano_tc_tx_buffer[5] = ctrl_rol >> 8;
    ano_tc_tx_buffer[6] = ctrl_pit & 0xFF;
    ano_tc_tx_buffer[7] = ctrl_pit >> 8;
    ano_tc_tx_buffer[8] = ctrl_thr & 0xFF;
    ano_tc_tx_buffer[9] = ctrl_thr >> 8;
    ano_tc_tx_buffer[10] = ctrl_yaw & 0xFF;
    ano_tc_tx_buffer[11] = ctrl_yaw >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[12], &ano_tc_tx_buffer[13]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送GPS传感器信息1数据
void ano_tc_send_gps_sensor_info1(uint8_t fix_sta, uint8_t s_num, int32_t lng, int32_t lat, int32_t alt_gps, int16_t n_spe, int16_t e_spe, int16_t d_spe, uint16_t pdop, uint16_t sacc, uint16_t vacc)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x30;
    ano_tc_tx_buffer[3] = 23;
    ano_tc_tx_buffer[4] = fix_sta;
    ano_tc_tx_buffer[5] = s_num;
    ano_tc_tx_buffer[6] = lng & 0xFF;
    ano_tc_tx_buffer[7] = (lng >> 8) & 0xFF;
    ano_tc_tx_buffer[8] = (lng >> 16) & 0xFF;
    ano_tc_tx_buffer[9] = (lng >> 24) & 0xFF;
    ano_tc_tx_buffer[10] = lat & 0xFF;
    ano_tc_tx_buffer[11] = (lat >> 8) & 0xFF;
    ano_tc_tx_buffer[12] = (lat >> 16) & 0xFF;
    ano_tc_tx_buffer[13] = (lat >> 24) & 0xFF;
    ano_tc_tx_buffer[14] = alt_gps & 0xFF;
    ano_tc_tx_buffer[15] = (alt_gps >> 8) & 0xFF;
    ano_tc_tx_buffer[16] = (alt_gps >> 16) & 0xFF;
    ano_tc_tx_buffer[17] = (alt_gps >> 24) & 0xFF;
    ano_tc_tx_buffer[18] = n_spe & 0xFF;
    ano_tc_tx_buffer[19] = n_spe >> 8;
    ano_tc_tx_buffer[20] = e_spe & 0xFF;
    ano_tc_tx_buffer[21] = e_spe >> 8;
    ano_tc_tx_buffer[22] = d_spe & 0xFF;
    ano_tc_tx_buffer[23] = d_spe >> 8;
    ano_tc_tx_buffer[24] = pdop & 0xFF;
    ano_tc_tx_buffer[25] = pdop >> 8;
    ano_tc_tx_buffer[26] = sacc & 0xFF;
    ano_tc_tx_buffer[27] = sacc >> 8;
    ano_tc_tx_buffer[28] = vacc & 0xFF;
    ano_tc_tx_buffer[29] = vacc >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[30], &ano_tc_tx_buffer[31]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 发送原始光流信息数据
void ano_tc_send_raw_optical_flow_info(uint8_t type, int16_t dx, int16_t dy, uint8_t qua)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x31;
    ano_tc_tx_buffer[3] = 6;
    ano_tc_tx_buffer[4] = type;
    ano_tc_tx_buffer[5] = dx & 0xFF;
    ano_tc_tx_buffer[6] = dx >> 8;
    ano_tc_tx_buffer[7] = dy & 0xFF;
    ano_tc_tx_buffer[8] = dy >> 8;
    ano_tc_tx_buffer[9] = qua;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[10], &ano_tc_tx_buffer[11]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}
// 发送通用位置型传感器数据(非捷联载体测量型)
void ano_tc_send_general_position_sensor_data(int32_t pos_x, int32_t pos_y, int32_t pos_z)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x32;
    ano_tc_tx_buffer[3] = 12;
    ano_tc_tx_buffer[4] = pos_x & 0xFF;
    ano_tc_tx_buffer[5] = (pos_x >> 8) & 0xFF;
    ano_tc_tx_buffer[6] = (pos_x >> 16) & 0xFF;
    ano_tc_tx_buffer[7] = (pos_x >> 24) & 0xFF;
    ano_tc_tx_buffer[8] = pos_y & 0xFF;
    ano_tc_tx_buffer[9] = (pos_y >> 8) & 0xFF;
    ano_tc_tx_buffer[10] = (pos_y >> 16) & 0xFF;
    ano_tc_tx_buffer[11] = (pos_y >> 24) & 0xFF;
    ano_tc_tx_buffer[12] = pos_z & 0xFF;
    ano_tc_tx_buffer[13] = (pos_z >> 8) & 0xFF;
    ano_tc_tx_buffer[14] = (pos_z >> 16) & 0xFF;
    ano_tc_tx_buffer[15] = (pos_z >> 24) & 0xFF;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[16], &ano_tc_tx_buffer[17]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析通用位置型传感器数据(非捷联载体测量型)
void ano_tc_parse_general_position_sensor_data(uint8_t *rx_buffer, int32_t *pos_x, int32_t *pos_y, int32_t *pos_z)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x32 || rx_buffer[3] != 12)
    {
        *pos_x = 0;
        *pos_y = 0;
        *pos_z = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[16] || addcheck != rx_buffer[17])
    {
        *pos_x = 0;
        *pos_y = 0;
        *pos_z = 0;
        return;
    }
    *pos_x = (rx_buffer[4] << 24) | (rx_buffer[5] << 16) | (rx_buffer[6] << 8) | rx_buffer[7];
    *pos_y = (rx_buffer[8] << 24) | (rx_buffer[9] << 16) | (rx_buffer[10] << 8) | rx_buffer[11];
    *pos_z = (rx_buffer[12] << 24) | (rx_buffer[13] << 16) | (rx_buffer[14] << 8) | rx_buffer[15];
}

// 发送通用速度型传感器数据(捷联载体测量型)
void ano_tc_send_general_speed_sensor_data(int16_t speed_x, int16_t speed_y, int16_t speed_z)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x33;
    ano_tc_tx_buffer[3] = 6;
    ano_tc_tx_buffer[4] = speed_x & 0xFF;
    ano_tc_tx_buffer[5] = speed_x >> 8;
    ano_tc_tx_buffer[6] = speed_y & 0xFF;
    ano_tc_tx_buffer[7] = speed_y >> 8;
    ano_tc_tx_buffer[8] = speed_z & 0xFF;
    ano_tc_tx_buffer[9] = speed_z >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[10], &ano_tc_tx_buffer[11]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析通用速度型传感器数据(捷联载体测量型)
void ano_tc_parse_general_speed_sensor_data(uint8_t *rx_buffer, int16_t *speed_x, int16_t *speed_y, int16_t *speed_z)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x33 || rx_buffer[3] != 6)
    {
        *speed_x = 0;
        *speed_y = 0;
        *speed_z = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[10] || addcheck != rx_buffer[11])
    {
        *speed_x = 0;
        *speed_y = 0;
        *speed_z = 0;
        return;
    }
    *speed_x = (rx_buffer[4] << 8) | rx_buffer[5];
    *speed_y = (rx_buffer[6] << 8) | rx_buffer[7];
    *speed_z = (rx_buffer[8] << 8) | rx_buffer[9];
}

// 发送通用测距传感器数据(捷联载体测量型)
void ano_tc_send_general_ranging_sensor_data(uint16_t direction, uint16_t angle, uint32_t dist)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x34;
    ano_tc_tx_buffer[3] = 7;
    ano_tc_tx_buffer[4] = direction & 0xFF;
    ano_tc_tx_buffer[5] = direction >> 8;
    ano_tc_tx_buffer[6] = angle & 0xFF;
    ano_tc_tx_buffer[7] = angle >> 8;
    ano_tc_tx_buffer[8] = dist & 0xFF;
    ano_tc_tx_buffer[9] = (dist >> 8) & 0xFF;
    ano_tc_tx_buffer[10] = (dist >> 16) & 0xFF;
    ano_tc_tx_buffer[11] = (dist >> 24) & 0xFF;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[12], &ano_tc_tx_buffer[13]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析通用测距传感器数据(捷联载体测量型)
void ano_tc_parse_general_ranging_sensor_data(uint8_t *rx_buffer, uint16_t *direction, uint16_t *angle, uint32_t *dist)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x34 || rx_buffer[3] != 7)
    {
        *direction = 0;
        *angle = 0;
        *dist = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[12] || addcheck != rx_buffer[13])
    {
        *direction = 0;
        *angle = 0;
        *dist = 0;
        return;
    }
    *direction = (rx_buffer[4] << 8) | rx_buffer[5];
    *angle = (rx_buffer[6] << 8) | rx_buffer[7];
    *dist = (rx_buffer[8] << 24) | (rx_buffer[9] << 16) | (rx_buffer[10] << 8) | rx_buffer[11];
}

// 发送通用图像特征点信息帧
void ano_tc_send_general_image_feature_point_info(uint8_t id, int16_t x, int16_t y, uint16_t angle)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x35;
    ano_tc_tx_buffer[3] = 7;
    ano_tc_tx_buffer[4] = id;
    ano_tc_tx_buffer[5] = x & 0xFF;
    ano_tc_tx_buffer[6] = x >> 8;
    ano_tc_tx_buffer[7] = y & 0xFF;
    ano_tc_tx_buffer[8] = y >> 8;
    ano_tc_tx_buffer[9] = angle & 0xFF;
    ano_tc_tx_buffer[10] = angle >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[11], &ano_tc_tx_buffer[12]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析通用图像特征点信息帧
void ano_tc_parse_general_image_feature_point_info(uint8_t *rx_buffer, uint8_t *id, int16_t *x, int16_t *y, uint16_t *angle)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x35 || rx_buffer[3] != 7)
    {
        *id = 0;
        *x = 0;
        *y = 0;
        *angle = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[11] || addcheck != rx_buffer[12])
    {
        *id = 0;
        *x = 0;
        *y = 0;
        *angle = 0;
        return;
    }
    *id = rx_buffer[4];
    *x = (rx_buffer[5] << 8) | rx_buffer[6];
    *y = (rx_buffer[7] << 8) | rx_buffer[8];
    *angle = (rx_buffer[9] << 8) | rx_buffer[10];
}

// 发送遥控器数据
void ano_tc_send_remote_control_data(int16_t rol, int16_t pit, int16_t thr, int16_t yaw, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x40;
    ano_tc_tx_buffer[3] = 20;
    ano_tc_tx_buffer[4] = rol & 0xFF;
    ano_tc_tx_buffer[5] = rol >> 8;
    ano_tc_tx_buffer[6] = pit & 0xFF;
    ano_tc_tx_buffer[7] = pit >> 8;
    ano_tc_tx_buffer[8] = thr & 0xFF;
    ano_tc_tx_buffer[9] = thr >> 8;
    ano_tc_tx_buffer[10] = yaw & 0xFF;
    ano_tc_tx_buffer[11] = yaw >> 8;
    ano_tc_tx_buffer[12] = aux1 & 0xFF;
    ano_tc_tx_buffer[13] = aux1 >> 8;
    ano_tc_tx_buffer[14] = aux2 & 0xFF;
    ano_tc_tx_buffer[15] = aux2 >> 8;
    ano_tc_tx_buffer[16] = aux3 & 0xFF;
    ano_tc_tx_buffer[17] = aux3 >> 8;
    ano_tc_tx_buffer[18] = aux4 & 0xFF;
    ano_tc_tx_buffer[19] = aux4 >> 8;
    ano_tc_tx_buffer[20] = aux5 & 0xFF;
    ano_tc_tx_buffer[21] = aux5 >> 8;
    ano_tc_tx_buffer[22] = aux6 & 0xFF;
    ano_tc_tx_buffer[23] = aux6 >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[24], &ano_tc_tx_buffer[25]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析遥控器数据
void ano_tc_parse_remote_control_data(uint8_t *rx_buffer, int16_t *rol, int16_t *pit, int16_t *thr, int16_t *yaw, int16_t *aux1, int16_t *aux2, int16_t *aux3, int16_t *aux4, int16_t *aux5, int16_t *aux6)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x40 || rx_buffer[3] != 20)
    {
        *rol = 0;
        *pit = 0;
        *thr = 0;
        *yaw = 0;
        *aux1 = 0;
        *aux2 = 0;
        *aux3 = 0;
        *aux4 = 0;
        *aux5 = 0;
        *aux6 = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[24] || addcheck != rx_buffer[25])
    {
        *rol = 0;
        *pit = 0;
        *thr = 0;
        *yaw = 0;
        *aux1 = 0;
        *aux2 = 0;
        *aux3 = 0;
        *aux4 = 0;
        *aux5 = 0;
        *aux6 = 0;
        return;
    }
    *rol = (rx_buffer[4] << 8) | rx_buffer[5];
    *pit = (rx_buffer[6] << 8) | rx_buffer[7];
    *thr = (rx_buffer[8] << 8) | rx_buffer[9];
    *yaw = (rx_buffer[10] << 8) | rx_buffer[11];
    *aux1 = (rx_buffer[12] << 8) | rx_buffer[13];
    *aux2 = (rx_buffer[14] << 8) | rx_buffer[15];
    *aux3 = (rx_buffer[16] << 8) | rx_buffer[17];
    *aux4 = (rx_buffer[18] << 8) | rx_buffer[19];
    *aux5 = (rx_buffer[20] << 8) | rx_buffer[21];
    *aux6 = (rx_buffer[22] << 8) | rx_buffer[23];
}
// 发送实时控制帧
void ano_tc_send_real_time_control(int16_t ctrl_rol, int16_t ctrl_pit, int16_t ctrl_thr, int16_t ctrl_yawdps,
                                   int16_t ctrl_spd_x, int16_t ctrl_spd_y, int16_t ctrl_spd_z)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x41;
    ano_tc_tx_buffer[3] = 14;
    ano_tc_tx_buffer[4] = ctrl_rol & 0xFF;
    ano_tc_tx_buffer[5] = ctrl_rol >> 8;
    ano_tc_tx_buffer[6] = ctrl_pit & 0xFF;
    ano_tc_tx_buffer[7] = ctrl_pit >> 8;
    ano_tc_tx_buffer[8] = ctrl_thr & 0xFF;
    ano_tc_tx_buffer[9] = ctrl_thr >> 8;
    ano_tc_tx_buffer[10] = ctrl_yawdps & 0xFF;
    ano_tc_tx_buffer[11] = ctrl_yawdps >> 8;
    ano_tc_tx_buffer[12] = ctrl_spd_x & 0xFF;
    ano_tc_tx_buffer[13] = ctrl_spd_x >> 8;
    ano_tc_tx_buffer[14] = ctrl_spd_y & 0xFF;
    ano_tc_tx_buffer[15] = ctrl_spd_y >> 8;
    ano_tc_tx_buffer[16] = ctrl_spd_z & 0xFF;
    ano_tc_tx_buffer[17] = ctrl_spd_z >> 8;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[18], &ano_tc_tx_buffer[19]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析实时控制帧
void ano_tc_parse_real_time_control(uint8_t *rx_buffer, int16_t *ctrl_rol, int16_t *ctrl_pit, int16_t *ctrl_thr,
                                    int16_t *ctrl_yawdps, int16_t *ctrl_spd_x, int16_t *ctrl_spd_y, int16_t *ctrl_spd_z)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x41 || rx_buffer[3] != 14)
    {
        *ctrl_rol = 0;
        *ctrl_pit = 0;
        *ctrl_thr = 0;
        *ctrl_yawdps = 0;
        *ctrl_spd_x = 0;
        *ctrl_spd_y = 0;
        *ctrl_spd_z = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[18] || addcheck != rx_buffer[19])
    {
        *ctrl_rol = 0;
        *ctrl_pit = 0;
        *ctrl_thr = 0;
        *ctrl_yawdps = 0;
        *ctrl_spd_x = 0;
        *ctrl_spd_y = 0;
        *ctrl_spd_z = 0;
        return;
    }
    *ctrl_rol = (rx_buffer[4] << 8) | rx_buffer[5];
    *ctrl_pit = (rx_buffer[6] << 8) | rx_buffer[7];
    *ctrl_thr = (rx_buffer[8] << 8) | rx_buffer[9];
    *ctrl_yawdps = (rx_buffer[10] << 8) | rx_buffer[11];
    *ctrl_spd_x = (rx_buffer[12] << 8) | rx_buffer[13];
    *ctrl_spd_y = (rx_buffer[14] << 8) | rx_buffer[15];
    *ctrl_spd_z = (rx_buffer[16] << 8) | rx_buffer[17];
}

// 发送匿名光流数据
void ano_tc_send_anonymous_optical_flow_data(uint8_t mode, uint8_t state, int16_t dx, int16_t dy, uint8_t quality)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x51;
    if (mode == 0)
    {
        ano_tc_tx_buffer[3] = 5;
    }
    else if (mode == 1)
    {
        ano_tc_tx_buffer[3] = 5;
    }
    else if (mode == 2)
    {
        ano_tc_tx_buffer[3] = 9;
    }
    else
    {
        return;
    }
    ano_tc_tx_buffer[4] = mode;
    ano_tc_tx_buffer[5] = state;
    if (mode == 0)
    {
        ano_tc_tx_buffer[6] = (int8_t)dx;
        ano_tc_tx_buffer[7] = (int8_t)dy;
    }
    else
    {
        ano_tc_tx_buffer[6] = dx & 0xFF;
        ano_tc_tx_buffer[7] = dx >> 8;
        ano_tc_tx_buffer[8] = dy & 0xFF;
        ano_tc_tx_buffer[9] = dy >> 8;
        if (mode == 2)
        {
            // 假设此处dx_fix、dy_fix、integ_x、integ_y为0，实际使用时需替换
            ano_tc_tx_buffer[10] = 0;
            ano_tc_tx_buffer[11] = 0;
            ano_tc_tx_buffer[12] = 0;
            ano_tc_tx_buffer[13] = 0;
        }
    }
    ano_tc_tx_buffer[ano_tc_tx_buffer[3] - 1] = quality;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[ano_tc_tx_buffer[3] + 4 - 1], &ano_tc_tx_buffer[ano_tc_tx_buffer[3] + 5 - 1]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析匿名光流数据
void ano_tc_parse_anonymous_optical_flow_data(uint8_t *rx_buffer, uint8_t *mode, uint8_t *state, int16_t *dx, int16_t *dy,
                                              uint8_t *quality)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x51 || (rx_buffer[3] != 5 && rx_buffer[3] != 9))
    {
        *mode = 0;
        *state = 0;
        *dx = 0;
        *dy = 0;
        *quality = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[rx_buffer[3] + 4] || addcheck != rx_buffer[rx_buffer[3] + 5])
    {
        *mode = 0;
        *state = 0;
        *dx = 0;
        *dy = 0;
        *quality = 0;
        return;
    }
    *mode = rx_buffer[4];
    *state = rx_buffer[5];
    if (*mode == 0)
    {
        *dx = (int8_t)rx_buffer[6];
        *dy = (int8_t)rx_buffer[7];
    }
    else
    {
        *dx = (rx_buffer[6] << 8) | rx_buffer[7];
        *dy = (rx_buffer[8] << 8) | rx_buffer[9];
        if (*mode == 2)
        {
            // 可根据实际需求处理dx_fix、dy_fix、integ_x、integ_y
        }
    }
    *quality = rx_buffer[rx_buffer[3] - 1];
}

// 发送航点读取帧
void ano_tc_send_waypoint_read(uint8_t num)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x60;
    ano_tc_tx_buffer[3] = 1;
    ano_tc_tx_buffer[4] = num;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[5], &ano_tc_tx_buffer[6]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}

// 解析航点读取返回帧（航点写入、航点读取返回帧ID均为0x61，此处解析读取返回部分）
void ano_tc_parse_waypoint_read_return(uint8_t *rx_buffer, uint8_t *num, int32_t *lng, int32_t *lat, int32_t *alt,
                                       uint16_t *spd, uint16_t *yaw, uint8_t *fun, uint8_t *cmd1, uint8_t *cmd2,
                                       uint8_t *cmd3, uint8_t *cmd4)
{
    if (rx_buffer[0] != 0xAA || rx_buffer[2] != 0x61 || rx_buffer[3] != 22)
    {
        *num = 0;
        *lng = 0;
        *lat = 0;
        *alt = 0;
        *spd = 0;
        *yaw = 0;
        *fun = 0;
        *cmd1 = 0;
        *cmd2 = 0;
        *cmd3 = 0;
        *cmd4 = 0;
        return;
    }
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    ano_tc_cal_sumcheck_and_addcheck(&sumcheck, &addcheck);
    if (sumcheck != rx_buffer[26] || addcheck != rx_buffer[27])
    {
        *num = 0;
        *lng = 0;
        *lat = 0;
        *alt = 0;
        *spd = 0;
        *yaw = 0;
        *fun = 0;
        *cmd1 = 0;
        *cmd2 = 0;
        *cmd3 = 0;
        *cmd4 = 0;
        return;
    }
    *num = rx_buffer[4];
    *lng = (rx_buffer[5] << 24) | (rx_buffer[6] << 16) | (rx_buffer[7] << 8) | rx_buffer[8];
    *lat = (rx_buffer[9] << 24) | (rx_buffer[10] << 16) | (rx_buffer[11] << 8) | rx_buffer[12];
    *alt = (rx_buffer[13] << 24) | (rx_buffer[14] << 16) | (rx_buffer[15] << 8) | rx_buffer[16];
    *spd = (rx_buffer[17] << 8) | rx_buffer[18];
    *yaw = (rx_buffer[19] << 8) | rx_buffer[20];
    *fun = rx_buffer[21];
    *cmd1 = rx_buffer[22];
    *cmd2 = rx_buffer[23];
    *cmd3 = rx_buffer[24];
    *cmd4 = rx_buffer[25];
}

// 发送航点写入帧
void ano_tc_send_waypoint_write(uint8_t num, int32_t lng, int32_t lat, int32_t alt, uint16_t spd, uint16_t yaw,
                                uint8_t fun, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3, uint8_t cmd4)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0x61;
    ano_tc_tx_buffer[3] = 22;
    ano_tc_tx_buffer[4] = num;
    ano_tc_tx_buffer[5] = (lng >> 24) & 0xFF;
    ano_tc_tx_buffer[6] = (lng >> 16) & 0xFF;
    ano_tc_tx_buffer[7] = (lng >> 8) & 0xFF;
    ano_tc_tx_buffer[8] = lng & 0xFF;
    ano_tc_tx_buffer[9] = (lat >> 24) & 0xFF;
    ano_tc_tx_buffer[10] = (lat >> 16) & 0xFF;
    ano_tc_tx_buffer[11] = (lat >> 8) & 0xFF;
    ano_tc_tx_buffer[12] = lat & 0xFF;
    ano_tc_tx_buffer[13] = (alt >> 24) & 0xFF;
    ano_tc_tx_buffer[14] = (alt >> 16) & 0xFF;
    ano_tc_tx_buffer[15] = (alt >> 8) & 0xFF;
    ano_tc_tx_buffer[16] = alt & 0xFF;
    ano_tc_tx_buffer[17] = (spd >> 8) & 0xFF;
    ano_tc_tx_buffer[18] = spd & 0xFF;
    ano_tc_tx_buffer[19] = (yaw >> 8) & 0xFF;
    ano_tc_tx_buffer[20] = yaw & 0xFF;
    ano_tc_tx_buffer[21] = fun;
    ano_tc_tx_buffer[22] = cmd1;
    ano_tc_tx_buffer[23] = cmd2;
    ano_tc_tx_buffer[24] = cmd3;
    ano_tc_tx_buffer[25] = cmd4;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[26], &ano_tc_tx_buffer[27]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}
// 发送功能触发命令帧
void ano_tc_send_function_trigger_command(uint8_t cid, uint8_t cmd0, uint8_t cmd1, uint8_t cmd2, uint8_t cmd3,
                                          uint8_t cmd4, uint8_t cmd5, uint8_t cmd6, uint8_t cmd7, uint8_t cmd8,
                                          uint8_t cmd9)
{
    ano_tc_tx_buffer[0] = 0xAA;
    ano_tc_tx_buffer[1] = 0xAF;
    ano_tc_tx_buffer[2] = 0xE0;
    ano_tc_tx_buffer[3] = 11;
    ano_tc_tx_buffer[4] = cid;
    ano_tc_tx_buffer[5] = cmd0;
    ano_tc_tx_buffer[6] = cmd1;
    ano_tc_tx_buffer[7] = cmd2;
    ano_tc_tx_buffer[8] = cmd3;
    ano_tc_tx_buffer[9] = cmd4;
    ano_tc_tx_buffer[10] = cmd5;
    ano_tc_tx_buffer[11] = cmd6;
    ano_tc_tx_buffer[12] = cmd7;
    ano_tc_tx_buffer[13] = cmd8;
    ano_tc_tx_buffer[14] = cmd9;
    ano_tc_cal_sumcheck_and_addcheck(&ano_tc_tx_buffer[15], &ano_tc_tx_buffer[16]);
    ano_tc_send_frame(ano_tc_tx_buffer[3] + 5);
}
// void ano_tc_handle_read(char *str, uint8_t len)
// {
//     if(str[0] == 0xAA && str[1] == 0xFF &&)
//     {
//         ano_tc_rx_buffer[ano_tc_rx_buffer_index++] = str[2];
//         ano_tc_rx_buffer[ano_tc_rx_buffer_index++] = str[3];
//         if(ano_tc_rx_buffer_index == len)
//         {
//             ano_tc_rx_buffer_index = 0;
// }
